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ABSTRACT 

A pure pursuit guidance law is combined with a heading 
autopilot to provide accurate path keeping of submersible 
vehicles. The scheme is implemented and analyzed in both the 
horizontal and vertical planes. A complete Stability analysis 
1s performed in order to evaluate regions of stable vehicle 
operations. Numerical integrations support the analytic 
maeadictions. Two Gs = incr Spabaglaey boundaries are 
Pereaolisned. Im the first, the vehicle loss of stability is 
accompanied by the generation of oscillatory motions around 
the commanded path. In the second, loss of stability occurs 
with linearly increasing path deviation. The horizontal and 
vertical plane schemes are combined with a propulsion control 
law in order to achieve path tracking of a general commanded 
route composed of several straight line segments in three 


dimensional space. 
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I INTRODUCTION 

One of the most significant functions of an underwater 
meepele —iS accurate path control for transiting along 
prescribed routes in three dimensional space. The commanded 
path is usually described by a series of way points in space 
and time either by the commander or by a path planner function 
in the case of an unmanned vehicle. Without significant loss 
of generality we can assume that the commanded path can be 
approximated by straight line segments between consecutive way 
points. This assumption does not alter the important features 
Of the path keeping problem since every smooth path can be 
approximated arbitrarily closely by a series of straight line 
segments. Once a desired straight line path has been 
generated, the vehicle guidance and autopilot functions are 
called upon to ensure satisfactory path keeping through the 
use of the vehicle actuators. 

One way to ensure that the vehicle goes through a 
Specified sequence of way points is by using a heading 
autopilot coupled with a line of sight guidance scheme [1]. 
The scheme proved to be robust enough so that when coupled 
with an independently developed depth autopilot [2], accurate 
depth control was maintained while transiting between way 
points in the horizontal plane. The disadvantage associated 


foem this technique is that the actual vehicle path between 


two consecutive way points differ significantly from the 
corresponding straight line segment. 

In order to overcome this problem and achieve accurate 
path control in the presence of obstacles and underwater 
currents, a cross track error autopilot was developed for the 
horizontal [3] as well as the combined horizontal and verureem 
planes [4]. A cross track error autopilot incorporates the 
deviation of the assumed straight line path into the control 
law design. This requires the introduction of additional 
kinematic relations in the control design and, as a result, 
the controller tends to be more sensitive to actual system / 
mathematical model mismatch. 

The main drawback of a cross track error autopilot is thae 
it represents a combined guidance / control scheme with no 
clear distinction between these two functions. Thus it is very 
vehicle specific and offers little flexibility in the design: 
Path control is limited to cross track error only and analysis 
of alternate schemes [5] is not possible unless the combined 
scheme is redesigned. For this reason we decide to separate 
once more the guidance and autopilot functions of the vehicle. 
An orientation controller is designed in order to provide 
accurate vehicle headings in response to guidance commands. 
The controller is, thus, based on the vehicle dGynangeam 
equations and Euler angle rates. A guidance scheme is used to 
provide appropriate heading commands through the kinematic 


equations of inertial position rates. A line of sight guidance 


command law is employed as in [6] and [7]. We consider a 
reference point that is moving ahead of the vehicle at a 
constant distance on the desired straight line path. We refer 
tO this distance as the lookahead distance. The commanded 
heading is then equal to the line of sight angle between the 
center of the vehicle and the lookahead point. By suitably 
selecting the lookahead distance the degree of convergence of 
the guidance law can be varied from very slow to very rapid 
onto the straight line path. 

Although the above scheme appears to be trouble free on 
the surface, a significant complication arises in the case of 
underwater vehicles. Since the actual vehicle response is 
relatively slow as dominated by the existence of important 
dynamical lags there is the possibility of instability when 
the guidance and control functions are combined. High values 
of the lookahead distance result in very slow vehicle 
response. The problem is then to evaluate these regions of 
Stable and unstable vehicle response. Chapter II of this 
thesis summarizes the stability analysis results for the 
horizontal plane. In Chapter III we proceed with the analysis 
of motions under the guidance and control scheme for the 
vertical plane. It is shown that the existence of hydrostatic 
restoring moments here due to the nonzero (positive) 
metacentric height brings in an additional form of instability 
not present in the horizontal plane. Finally in Chapter IV the 


previous two guidance and control schemes for the horizontal 


and vertical planes are combined and with a speed) aute@pa ieee 
accurate path tracking in three dimensional space is achieved. 
The main conclusion of this work is that guidance and control 
laws for underwater vehicles must be designed together even if 
they are kept separated, in order to ensure stablewiaue 
satisfactory path keeping. All computations in this work are 
performed for the Swimmer Delivery Vehicle [8] for which a 
complete set of hydrodynamic coefficients and geometrve 


properties is available. 


II. HORIZONTAL PLANE 
In this section the vehicle equations of motion for the 
horizontal plane (x,y), the design of a heading autopilot and 


Simulations and stability results are presented. 


A. EQUATIONS OF MOTION 
For the horizontal plane the mathematical model consists 


of the nonlinear sway and yaw differential equations shown 


below: 
m(V+ur+xX,F-y,r*) =Y (2.1) 
T,r+mxX,(V+ur) -my,ur=N (2-2) 
EQuations (2.1),(2.2) can be easily derived from the general 


six degrees of freedom equations for a vehicle by assuming all 
terms off the horizontal plane to be zero. The equations for 
the sway force Y and yaw moment N are presented below: 


co an Guere e)- i 2 
Baa ate eee Lt YU Bf lcpB te) pea ] d&+Y,u*d 


a: ie _p Cae i 2 
N=N,0+(N,V+N_ur) +N uv Bf lena (8) zea €] dé+N,u°sd 


To complete the model, expressions of the ~inerieaam 
position rates and yaw rate are required. These jarem=ags 


kinematic equations: 


=r (2.3) 
X=ucosw-vsinw (2.4) 
y=usiny-vcosy (2.5) 


B. CONTROL LAW 


It is more convenient for the design of a linear state 
Space heading controller to represent the above equations 


(2.1),(2.2),(2.3) in the following foum (71 chy se 


W=r (2.6) 
Vea,,uv+a,,urt+b,u*b+d,(v, r) (2.7) 
r=a,,UV+a,,Ur+D,U- Ord. (Vv, 2) (2.8) 
nie Ger 
Dai, -N 7) (m= Y 5) ee i) ove 
a= [ (T,-N,) Y,- (mx,-¥,) N,] 


a,,7= | (m-Y,,) N,- (mx,-N,) Y,] 
a,,=— [ (m-¥,) (-mx,+N,) - (mxg-N,) (-m+Y,) ] 


‘ei 
d, (Vv, r) =-= = pCp [ (m-¥,) I, +N, Tp] 


T,={ (h(E) (v+Er) | (v+Er) |] dé 


eH [h(E) (v+Er) | (vbr) |B] dé 


PicenOnlinedr ~erms ad (v,r),d (u,r) are small and can be 
m@eqiected for control law design. They are kept, however, in 


all numerical simulations that follow. 


1. ZERO YAW ANGLE 


When the commanded yaw angle of the vehicle is zero the 


control law has the following form: 


6=k,W+k,v+k,r (2.35 


where k,,k,,k, are computed so the system will have the desired 
dynamics. The closed loop characteristic equation has the 


tollowing roan 


A? +a,A*%+a,A+a,=0 (2.10) 


where: 

= 2 2 

Qa, =a, , Uta UP Oy tee 
_ ? 7 _ a 2 
@,=(@,,4,,+4,,D,UK,+b,a,,UK, -—2\5,4a5,-4,,0,UK, a5, eee ee 
= = 2 

ay- | Dan aay Ul Ky 

The characteristic equation is specified in the following 


way. It can be chosen to satisfy the minimum ITAE criterion 


where it assumes the form: 


M3+a,A2+a,A+0,=0 (2. ae 


where: 





meme © epresents Che dimensionless settling time for the 
mascem. MQuating the coefficients of equation (2.10) with the 


desired equation (2.11) and after some algebra we find: 


ao (2.12) 
(b,4,,-D,a,,) ae 


k, (b,a,,-b,a,,) u°+k; (b,a,,-b,a,,) u°=a,+b,u°k, (2.13) 


ip D,u-+K,D,u-=-a,-(a,,4+4,5) u (2.14) 


eo ceomemarvalue tomer, according tothe ITA criterion, 
dictates complex conjugate dominant poles with oscillatory 
transient response. It was found that other poles selections 
(for example real negative) do not change significantly the 
nature of the results and the stability boundaries that are 


presented later. 
2. NON ZERO YAW ANGLE 
If the commanded yaw angle is non zero and equal to  ., 


then the control law (2.9) is simply modified to: 


Og ni Wie) te ete re (2.15) 


xq 


ae ; 


Ty COU 





Figure 1. Horizontal plane geometry 
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No feedforward term is necessary in (2.15) since no rudder 
angle is required to keep the vehicle to a constant non zero 


heading angle at steady state. 


C. GUIDANCE 

The heading autopilot that was designed in the previous 
section is called upon now to provide vehicle path in the 
sense of passing through a series of way points in the 
horizontal plane. In order to achieve it without changing the 
previously designed heading autopilot we have to couple it 
with a suitable navigation scheme such as line of sight 
guidance. 

The simplest such guidance law iS a pure pursuit 
navigation which is accomplished as follows. The autopilot 
attempts to point the longitudinal axis of the vehicle towards 
meeoimnt Db which is located ahead to the vehicle on the nominal 
Seeargnt line path at a fixed distance x, as shown in Figure 
feuies tCangee distance x, to as the visibility, lookahead, or 
preview distance. The line of sight angle o is defined by: 


tano=-— (2.16) 
Xq 


Pure pursuit navigation then corresponds to taking: 


Woe (2.17) 


ee 


as the commanded heading angle in the control law (2.15). 

It can be seen now that the commanded vehicle Nheagaaae 
angle is not constant but it is function of the vehicle 
position y. This introduces the lateral deviation equation 
(2.5) into the problem, and since the control law was based on 
equations (2.6),(2.7) and (2.8) only, Stability ope 
combined autopilot-guidance scheme is no longer guaranteed. 
Therefore, we need to develop conditions which will guarantee 


Stability and ensure satisfactory path keeping. 


D. STABILITY 

The complete system is given by the differential equations 
(2.6),(2.7),(2.8), the» control law (2.15), and the iveanee 
equations (2.16),(2.17). The trivial equilibrium epee 


corresponding to a straightweline motion is characterizea@age 


W=v=r=y=0 


Linearization of the state equations gives the following 


linear system: 


where the complete state vector is; 


X=(w, Veal 


AZ 


Local stability properties are established by the eigenvalues 


of [A] The characteristic equation is found to be: 


AA4+BA2+CA2+DA+E=0 (2.18) 
where: 
A=1 
B=-B,-C, 
C=-D,+B,C,-C,B,-A, 
D=-C,D,+D,C,-uD,-A,B,+A,B, 
and 


An Die K: 
A,=b,u*k, 
Bee any ua, ak. 
Baan Ds ticle 
eae Dek, 
C,=a,,u+b,u*k, 

il 


dD, =b,u°k, — 
d 


iS 


Ds =b,u*k, — 
d 


Loss of stability occurs when 


BCD-B*E-AD?=0 (2.19) 


Equation (2.19) is derived from Rooth's criterion £60 (2a 
and it corresponds to a pair of complex conjugate roots 
crossing the imaginary axis. After some algebra equation 


(2.199 1sS Ssimplitited sro. 


a, X$+a,X4ta,=0 (2.20) 


where: 
a,=%,4,-Q, 
a2 bh a.-ba. || (pio p ene 
2411791421 (b,4,,-D,a2,)u 
ee - (b, a,,-b,4,,—-2,) |b) 0ae (elas oa a ee 
oO  eE————————— 


(Ora ae ae 


The positive root of equation (2.20) determines the 
critical value of x, for stability. For every x) eee 
system is stable which means that the vehicle will follow the 


path. In the opposite case) whewewec a. the system 


Ky critical 
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becomes unstable and the motion of the vehicle becomes 
oscillatory as a result of a complex conjugate pair of 
eigenvalues with positive real parts. 

Results for the dimensionless critical visibility versus 
Peecencgecame to dre presented in Figure 2. These results are 
meaependent of the forward speed since gains k,,k,,k, are 
Mmeerions Of u. It can be seen from Figure 2 that for higher 


ieccerter controller) higher lookahead distance x, is required 


d 
in order for the system to remain stable. It is obvious that 
very high values of x, correspond to a very slow navigator 
with a loss in speed of response and navigational accuracy. 
The results of this section establish analytically the minimum 
required lookahead distance that is required for stability 
based on linear approximations. 

It should be mentioned that all results in this work are 
presented in dimensionless form unless otherwise mentioned. 


Nondimensionalizations are performed by using the vehicle 


length and the vehicle forward speed. 


iS 


ond 


4.0 


3.0 


Cy 
2.0 


. 0 


_ 
oO 
oO 
0, 000 0. 200 0.400 0. 600 0. 800 


—T rl 


Raq 


Figure 2. Regions of stability in the horizontal plane 
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E. SIMULATIONS 

Numerical simulations confirm the results of the stability 
analysis of Figure 2. The simulated lateral distance y (in 
vehicle lengths) versus time t (in dimensionless seconds) is 
Shown in Figure 3 for two cases. The nominal straight line 
path is y=0 Case 1 is located in Region 1 of Figure 2 and it 
can be seen that the vehicle response is unstable. Case 2 
Seeresponds LO a stable (t,,x,) combination and the vehicle 


converges to the desired path. 


JOY 
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Figure 3. 
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Stable and unstable numerical simulations 
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III. VERTICAL PLANE 
In this section the vehicle equations of motion for the 
vertical plane (x,z), the design of a vertical heading 


autopilot and simulations and stability results are presented. 


A. EQUATIONS OF MOTION 
Restricting our attention to the vertical plane the 
mathematical model consists of the nonlinear heave and pitch 


differential equations shown below: 


3.1 
BMA et le Dl ay 2) a) = ( 


I,qd-mxX,(w-uq) +mz,wq=M C322) 


where only vertical plane related terms have been kept. The 


heave force Z and pitch moment M are written as: 


3 
Z=Z49G+(Z.W+Z z.uw-2 fc. b(x AWHXD)” ayy W-B s6+ 
git (Z,i+Zquq) +Z,uw-© fey b(x) Woe 2S (WB) co 
u*(Z, 6,+Z5 9,) 


= 3 


-(Z,W-Z,B) sinO+u* (M, 5+, 6,) 


Ox aN ee) COSU 


In the above equations is the vehicle weight, B the buoyancy, 


meee) the coordinates of the center of gravity, and (X,,2Z,) 


rg 


the coordinates of the center of buoyancy. Also, provision for 
two sets of control surfaces (stern and bow planes) is made. 


The kinematic equations are: 


x=ucos0+wsin® (3.3) 
Z=-usinO+weos8 (3.4) 
6=g (3.5) 


B. CONTROL LAW 
The linearized state space form of equations (3.1),(3.2) 


and (3.5) is used for vertical plane heading control: 


W=@,,UWt+a,,Ud+a, 01D, 0 Olena (3.6) 
G=a,,UWta,,Uq+a,,0+0,,U-0 + Oya, (3.7) 
O=q 


where: 


a= [ (I,-M,) Z,+ (mxg+Z,) My] 
a,2=- U(1,-M,) (m+Zq) + (mxgtZq) (Mq-m) | 
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a,y2-— [(2,-Z,) (mxX_+Z,) W) 


Vv 


a= [ (£,-Mg) 255+ (MxXQt+Zy) My] 


= 
sD), 


S| 


< 


ao1= [ (m-Z,,) Mase (mx,+M,,) Z| 


i 
D, 


aj9=—~ ( (m-Z,) (M,-m) + (mx_+M,) (m+Z,) } 


Vv 


ayy=-— [ (is Zey) iam) W] 
b,.=— [ (m-Z,,) My ..+ (mx _tM,,) Zs] 


Vv 


b= [ (m-Z,) Myp+ (mxg+M,) Zep) 


a 


mimenese W=B and x.-x, have been assumed. Considering that the 


effect of the bow and the stern planes is the same we have: 


Za 


SO 


Db, =D,,-5,2 


b,=b,, ~b,, 


From the above the final form of the equations of momma 


Se 
O6=q 
W=a,,uw+a,,ugqra,,0+b,u26 (3.8) 
G=a,, uw+a,,uq+a,,0+b,u*d (3.9) 


ane ZERO PITCH ANGLE 
When the commanded direction of the underwater vehicle is 


horizontal the control law has the follewiuig ero 


6=k,6+k,w+k,g (3.10) 


where k,,k,,k, are calculated below. From the system Of jams 
three differential equations (3.5),(3.8),(3.9) the closed Hoge 


characteristic equation has the following form: 


A2+a,A?2+a,4+a,=0 (3.11) 


where: 


ZZ 


= — is Ee = 2 
eee Oks aU 
Ee 2 Z 2)e ie 3k Sexe = 
Pee ape ape iek, apa, —05a,,U K,-D,a,,U° K,-4,,-D,u 


_ 7 oe 3 3 2 
aca ayinas,0,0 Ko -0,2,,0 K,+2,,4a,,U+a,,D,U~K,+a,,D,u°k, 


2 


Ka 





Figure 4. Vertical plane geometry: Horizontal commanded path 
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The desired characteristic polynomial according to the ITAE 


@ieiterion is: 


A2+0,A7+0,A4+0,=0 (3-12) 


where: 


@,=1.75W, 


Cee raw, 





t=O 
10u 
®@,= 
ee] 


fomee, represents the dimensionless settling time for the 
vertical plane autopilot. Equating the coefficients of 


equation (3.11) with equation (3.12) we get: 
b,u*k,+b,u*k,=-0,-(a,,+a,,) u (3.13) 


an Saye mas a: a Fe 142 
(b,a,,-b,a,,) u2k,+ (b,a,,-b,a,,) uk, =a, +5, uk, 


Zoe 2 
= 2 3.14 
Cio a (2,545, €11455) u ( ) 


oe ae) hat) Day) 0 K>= 
(3.15) 
@,+ (4,34,,-4,,4,3) U 


Meesimplify notations, equations (3.13),(3.14) and (3.15) are 


written as: 


ao 


A, Kj A, ke 


B, Ki + Bok =e es 


CK, 7G eee 


B,=(b,a,,-b,a,,) uw 

B,=(b,a,,—-b,a,,) u° 

C= (b,a,,-b,az,) u° 

Cz = (a,,b,-@,3b,) u* 

D,=-0,- (-4,,+@,2/ U 
Dz=0,+42,+ (4124,1- 411422) Ue 


D, =, + (€,342,-4,,4,3) U 


can find expressions for the gains k,,k,,k, 
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(3.16) 


(3.17) 


(3.18) 


From the above system of equations (3.16),(3.17),(3.18) we 


k 3.19 
a (3.19) 
= A,B,D,+C,B;D,-D,C,A; (3.20) 
eee 
D,-A,K 
k= (3.2.1) 
A, 


2. NON ZERO PITCH ANGLE 


When the commanded pitch angle of the vehicle is not equal 


to zero, we have: 


0=a,+0 (er) 


where: ad 26 che commanded patch angle 


8' is the deviation from the commanded angle 


Then 


sin@=sina,cos0’+cosa,sin@’=sina,+0/cosa, 


for small deviations @'. The system of equations of motion 


meme), (3.8),(3.9) takes the form: 


es (228) 


/ 2 
W=a,,UW+a,,ug+a,,cosa,0’+b,u*5+a,,sina, (3.24) 


Pha 


> 2 


Figure 5. Vertical plane geometry: Inclined commanded path 
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a / 2 ® 
Gace cd .eOoa,u +O U-Oa,,Sina, (3.25) 


The control law now takes the form: 


6=k, (90-a,) +k,w+k,qtk, (3.26) 


where k,,k,,k, can be calculated with the some procedure as 
merore, and the feedforward gain k, is calculated from the 


desired steady state accuracy. At steady state we have: 


gq=0 


SO eiiel the system of the equations On motion 


jerezs),(3.24),(3.25) yields: 


a,,uw+b,u*6+a,,Sina,=0 (2727) 


a,,uw+b,u*6+a,,Ssina,=0 (3.28) 


memetrons (3.27),(3.28) can be solved for the steady state 
values of 6 and w, and by substitution into equation (3.26), 


after some calculations k, TS FOuUNGatOwbDe* 


29 


__ 43 (42, +b,Uk,) nay een sina, (3.29) 
(D,a2,~0,4,,) u* 


Note chat 15 “a —-Ovcim—eZ 2 


paeon then k,=0. 


C. GUIDANCE LAW 

A similar to the horizontal plane case guidance law can be 
used here to allow path keeping in the vertical plane. To the 
previous system of differential equations (3.1),(3.2),(3.5) 
one more equation is added, the kinematic equation (3.4). The 
new system is now going to be examined for two different 
cases. 

a) Horizontal path (no change in depth) 

b) Inclined path (change in depth) 

1. HORIZONTAL PATH 

In this case where the commanded depth remains the same 


the control law is: 


6=k, (6-8.) +k,w+k,g (3.30) 


where 9. is the commanded line of sight (pilech wands 


6.=tan *— (3.31) 
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where k,,k,,k, are already known from the previous section, and 
meromere visibility distance similar to the horizontal case, 
shown in Figure 4. 

2. INCLINED PATH 

Here the commanded depth changes linearly so that the 


angle 6 is given by: 


5=k, (6-a,-8' .) +k,w+k,q+k, (3.32) 


where k,,k,,k,,k, are the same as previously determined. 

Mies, term exists here because an angle 6 # 0 has to remain 
when the underwater vehicle changes depth to equalize the 
restoring moment due to the pitch angle. The commanded pitch 


angle is: 


/ 
Se =tane —— (3)23'5)) 
see 


where z is the cross track error off the inclined path as 


shown in Figure 5. 


D. STABILITY 

The complete system is given by the equations of motion 
meee (s.2), (3.4), (3.5), the control law (3.30), and the 
moraance law (3.31). Horizontal motion at the commanded depth 


1s characterized by: 


OW G= 2-0 


ol 


Linearization of the above equations produces the linear 


Syow Cll. 
Xan 
where: 
X= (6, W, qd; Zz] 
and 
Zs Za Z 2 Ky 
@13ZoptD,UcK, @),UtD UK, aa eee 
Be 
aie q| (3.388 
: : ; K. 
2542Ze,+b,U°K, a,,U+D,U °K ays Hone ee 
2 ee 
ae il 0 0 
where: 
Cpe ee (3 aioe 


is the metacentric height. Stability properties of the 
straight line motion are established by the eigenvalues of 
matrix [A]. It should be mentioned that from now until the end 
of this chapter a,,, a,, have been redefined to show exp licaiaame 
the Metaceneric Memo ieee. 

A program is written to compute the eigenvalues of matrix 


(3.34) over a range of (t,,x,) values, and detect whethereeme 
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or more eigenvalues become unstable. Typical results are shown 
toeengome 6 Longuesyit/sec and z,..=0.1- 
1. REGIONS OF STABILITY 
It can be seen that the stability boundary of Figure 6 
wepaedres the parameter space (x,,t,) into three regions: 
I-enstable Tegqion, sone paing of complex conjugate 
eigenvalues of [A] has positive real parts. 
2: Stable region, all eigevalues of [A] have negative 
real parts. 
3: Unstable region, one real positive eigenvalue of 
[A]. 
Obviously, stable vehicle response is not possible unless the 


Bemamerers (x,,t,) are chosen in region 2. 


a3 


1: One pair of complex conjugate eigevalues with 
positive real parts. 
2: Requaen of stable 


3: One real positive eigenvalue. 
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2. SIMULATIONS 

Before proceeding further with the stability analysis, 
numerical integrations are first performed in order to examine 
the response of the vehicle in each of the above three regions 
of stability of Figure 6. The same parameters u=5 ft/sec and 
meee) ft are used. Simulations for the pitch angle 6 and the 
commanded line of sight angle 0, for the case t,=5 and x,=4 are 
shown in Figure 7. This corresponds to region 2 of Figure 6 
which is the region of stability. The simulation results show 
that the actual vehicle pitch angle approaches the commanded 
angle, after some oscillations, and the depth reaches its 
commanded value at zero as predicted. 

Binen the visibility distance is x,=0.4 with the same t,, 
the vehicle moves into the unstable region 1 of Figure 6. The 
Simulated response is shown in Figure 8 where oscillatory 
characteristics are exhibited. If we keep the same value for 
me and we Change the controller time constant t,=15 we enter 
the unstable region 3 of Figure 6. The simulated vehicle 
meo@onse 15 Shown in Figure 9 where it appears that 9 and 0. 
diverge and they both reach nonzero steady state values. As a 
result the vehicle depth is now a linear function of time, 
without ever stabilizing. These results require a more 
detailed analysis of the regions of stability of the 


controller / guidance combination. 
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Figure 7. Numerical simulations in region 2 
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Figure 9. Numerical simulation in region 3 
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E. ANALYSIS 
The stability properties of the system are characterized 
by the eigenvalues of the linearized matrix [A]) Qivermuee 


equation (3.34). The characteristic equation of [A] Nastia 


form: 
AA4+BA2+CA2+DA+E=0 (3.36) 
where: 
A=i 
B=a, 


C=0,+b,u2k, 
Xq 





1 1 
Xq 5 


E'=Zo,(b,a,,-D,a23) uk + (D,a,,-D,a5;) ook 
Xq Xq 


According to Routh's criterion (3.36) has One pair Of oni 


conjugate roots crossing the imaginary axis when: 


BCD-AD? -B* E=0 (3.37) 


After some algebra , equation (3.37) can be written as 
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&,(4,0,-a,) x§t+[d, (a,a,-a,) +4,C,0,-a0,d,-afe,] (suse) 
Bal (Caran) =O) 


where: 
Ci =A,k, 
d, = ( -B, -A, u) iS 


e =e, u) 


and A,,A;,B,C,,C, were defined previously following equations 
eeto),(3.17) and (3.18). 

The positive root of equation (3.38) provides the critical 
fete OF xX), £Or stability. This produces the curve separating 
memreegqions 1 and 2 of Figure 6. As the (x,,t,) combinations 
cross into region 1, the response of the system becomes 
oscillatory as a result of the pair of complex conjugate 
eigenvalues with positive real part. This explains the 
Simulations observed in Figures 7 and 8. 

A different kind of instability occurs when one real root 


of (3.36) crosses zero. For this to happen the condition is: 


E=0 (3.39) 


and using the previous definition of E, this happens when 
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k,=0 (3.40) 


Equations#(3.40) and (3.959) eyed 


k= = (3.41) 


Equations (3.41),(3.20) define then the critical condition for 
stability. In our case this can be simplified as follows. The 


expression for C, is reprodieged@ iene 


C,= (45,0) aig) eee (3.42) 


Since we have assumed that bow and stern planes have the same 


strength 


Ms ,-=—-Ms,<0 


and substituting the expressions for €5,,D,,8,3;,b, we can fing 


Ghat 


C,=0 (3.43) 


Equations (3.43) and (3.41) then require that 
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D,=0 (3.44) 


and using the definition for D, we get 


065+ (4; 3221-41423) Zggu=0 


or 


iO 
rant 


Vv 


3 = 
+ (@1345,-4,,4,,) Zg_=0 


emeewe can find the critical value of = as 


100 


Voritical 


I { (€,,43,-€4,3,42,) Zp] 


(3.45) 


wie 


Memeriren (3.45) shows that the critical value of t, is 
ie@ependent Of x, which is demonstrated in Figure 6 as a 


straight line parallel to the x, axis. Furthermore, the other 


d 
Meeeeity Curve, equation (3.38), intersects the t, axis at 
xy=0 when k,=0 which is the same condition as (3.45). This 
means that the stability conditions (3.38) and (3.45) separate 
Miem(x,t ) Parameter space into three regions of stability, 
as shown in Figure 6. 

fPeaTtieomorstie Stability regions for 2..=0 are shown in 


Figure 10. These are independent of the forward speed u just 


as in the horizontal plane case. It should be mentioned that 
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iO = Ome 


GB > » and therefore ,region 3 Of fiGuiemeee 


veritical 
never appears. 
For zZ., > 0, the stability regions depend heavily \cnmiame 


forward speed u. This is demonstrated in Figure Il for 2Zegem 


G 
(ft) and various values of u in (ft/sec). As the speed is 
decreased the critical value of t, from (3.45) also decree 
with the effect of reducing region 1 and enlarging region 2 
ane 3a 


The effect of varying the metacentric height z,, while 


GB 
keeping u constant is evaluated in Figure 12 for u=2. Similar 
conclusions can be drawn for this case as previously. 

The critical value of t, as given by (3.45) 1s Showmieeg 
Figure 13 for different values of the forward speed u and the 


metacentric height z The surface shown in the figure 


GB° 
separates the stability regions 2 and 3. 
The final task of this section is to explain 


Simulations observed in Figure 9 when the vehicle operates in 


regions 3: 
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F. STEADY STATE SOLUTIONS 

It was shown in the previous section that transitaiem@ 
between Regions 2 and 3 is associated with a real eigenvalue 
crossing zero. Usually when such a loss of stability occurs 
and the primary equilibrium solution becomes unstable, 
additional stable equilibrium solutions appear. To evaluate 
these new steady state solutions we consider the complete 
system given by equations (3.4), (3.5), (3.6), (3.7), (3usgmm 


and (3.31). At steady state the time derivatives vanish anda. 


get 
g=0 (3.46) 
G 
v= oan (3.47) 
Cy 
D,-@, 
6=—-— sin® (3.48) 


Substituting equations (3.46),(3.47), and (3.48) into equauiven 


(3.4) we oGeE:; 


(tis 2 eo eeenoen (3.49) 
Cy 


Equation (3.49) may accept besides the normal solution 6=0, 


another solution given by: 
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= 2 
most] 2 puta P1421) UE (3.50) 
C, (b,2,,-b,4,3) 2 GB 


moewation (3.50) is valid provided cos® < 1 which means: 


(D,22,~b,4,,) Zep (3.51) 
b,4,,~b,a2, 


Ze 


If (3.51) is satisfied the equilibrium angle © can be 


determined from (3.50) provided: 


sin6<6 (3195 2 ) 





sat 


fleece oO . 1s the maximum dive plane angle typically set at 
0.4 radians. 

Pie oun Case conditions (3.51) and (3.52) are not 
satisfied, which means that the non zero equilibrium pitch 
ieee cannot be computed from (3.50). Furthermore z # 0 at 
meecay state, which means that z=constant. Therefore, z is 


tinearly increasing with time, and 


tan1—4-2 as... t-0 S255 
vn (3.53) 
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Substituting equations (3.46), (3.47), (3.48), and) (3.53 ae 
the control law (3.30) and (3.31) we can find the equationgeee 


the unknown steady state pitch angle. 


(D,-05) sin6=k,C, (0-+) +k,C,sin® (3.54) 


If we call 0 - nm/2 = 86', equation (3.54) reduces to: 


k,C,0’+ (a,-k,C,) cos8/=0 (3.55) 


It can now be seen that equation (3.55) has a solution when k, 
crosses zero which is the same condition for transition 
between regions (2) and (3) found in the previous Sectlon eee 


Steady state solution is then computed from (3.55) if: 





D,-@ ; 
= sin6<d,,, (3.56) 
Ol vir On. 
sin0= 73 = (3.57) 
3 3 
otherwise. 


Results for the steady values of 6 and 6 are presented in 


Figures 14 and 15 versus) 2., for =s anaes 


GB 


50 


Solid lines correspond to stable and dashed lines to unstable 
equilibrium positions. It can be seen that the simulation 
femmes fOr 2, —-0.1 Of Figure 9 are verified. 

The steady state pitch angle 8=0 loses its stability at 
meee. 07 and begins to increase together with the dive plane 
eagle Oo. This is up to 2Z..=0.12 where 6 reaches its maximum 
feeme-. FOr increasing 24... beyond this point, the pitch angle 
6 begins to decrease since 6 remains constant. These results 
faemecor fixed t, and u. Results for different values of the 
Gemecroller settling time €, and vehicle speed u are shown in 


Figures 16 and 17 respectively. 
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Figure 14. Steady state pitch angle 0 versus Z,, 
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Figure 16. Steady state 0 versus Z¢, for several values of t, 
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IV THREE DIMENSIONAL GUIDANCE CONTROL 
The horizontal and vertical plane guidance and control 
laws that were developed in the previous two chapters am. 
combined here to provide accurate path keeping in three 
dimensional space. The other requirement is that the forward 
speed along the path should be constant and equal to a 
commanded value. This will enable path tracking instead of 


Simply path keeping. 


A. PROPULSION CONTROL 

Just as the horizontal (vertical) plane path control 
design was based on the linearized lateral (vertical) 
equations of motion, the propulsion control law will be based 
on the linearized form of the surge equation only. The surge 


equation is: 


Mmu=X jU+X,,,W* +uwW(X,5 —X,5,) 8 +u* (Xs 5p) 5*+C,,(a*n*-u-) (fae 


where: 


o, = Tax (4.2) 


nis the propeller revolutions, and 6 the dive plane angle. 


Only w and 6 terms remain in equation (4.1) because only these 


56 


terms are nonzero at steady state for a constant commanded 
dive or rise angle. A propulsion control law is introduced of 


the form: 


n=n,+k,(u-u,) (4.3) 


iiie feedback @igain = kK “ism computed from stability 
requirements whereas the feedforward term n, is computed from 


steady state accuracy. When n=n, the forward speed u must 


0 


Eamee the commanded speed u.. Therefore, (4.1) becomes: 
Exe) + C, 0-1, -0 (4.4) 


where we defined 


£(u) =X,,,w*tuw(X,—3 -Xyp) 8+? (Xs 5 +X5 5.) 87-Cp ue (4.5) 


MitemcecrmS w and 56 are given as functions of u, and the 


Semmanded pitch angle a, by 


Wwe (D,a,,-b,a,;) ZogSina, (4.6) 


(2,,5,-a,,5,) u, 


(4.7) 


solving (4.4) for n, we get 
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ee” 


i 
0 





4.8 
Cy a ( 


This term nj, guarantees the required steady state accurecyauas 
evaluate k we substitute (4.3) and (4.8) into (4°72) anggae 


ger: 


(m-X,) U-2C, a? nk, (u-u,) =0 (4.9) 


The characteristic equation of (4.9) is 


=0 (4.10) 


The desired characteristic equation is 





ore 
ae. 


i 


S+O,=0,..W, (4.11) 


where t. is the desired dimensionless settling Cime =i Gmiiaas 
speed control. Comparing (4.10) with (4.11) we can solve for 
the contro! Gqaaa. 


5 Un (mG 
Cy a", bd 


k= 


Nn 


(4.125 
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With the choice of gains (4.12) and (4.8), the propulsion 


control law (4.3) is complete. 


B. THREE DIMENSIONAL PATH KEEPING 

Suppose the commanded path is a general straight line in 
three dimensions, from point O to point F as shown in Figure 
18. The vehicle position is at point A. With respect to the 
inertial coordinate frame (x,y,z) the commanded path is 
characterized with the two angles a, and a, as shown in the 
Mere. In Order to achieve the commanded path, a coordinate 
meame rotation by an angle a, 1s performed first as shown in 


Figure 19. The necessary geometric relations are: 


2 (4.13) 
a ,=tan #72 
Fo 
x’=(y-y,) sina,+ (x-x,) cosa, (4.14) 
Vie yinV) COsa.-( x= sina (4.15) 
The rudder control law is then of the fornm: 
em OG rl) ce eae (4.16) 


where the line of sight angle for horizontal plane control a, 


is defined by: 


a 


Cano,=- Ba 
Ky 
H 





(4.17) 


X,, 1s the lookahead distance determined according to the 


dH 
stability analysis of Chapter II, and k,, k,, k, ape 
horizontal plane control gains from Chapreecwaae 


Another rotation by an angle a, is conducted next asueijagm 


in Figure 20. The geometric relations here are: 





Zp Z 
a, =tan+—_2 (4.18) 

Ge 
x! = (Ver ,) Sane ean rele ecm (4.19) 
x"=-(z-z,) sina,+x'cosa, (4.20) 
z'=(Z-Z,) Cosa) saute (4.21) 


The dive plane control law is: 


6=K, (0-0 -o | iowa (4.22) 


where the line of sight angle for vertical plane Ccontro=G ae 


defined by: 


/ 
tano.= a 
a 
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feeeeeos Che lookahead distance determined according to the 
peeotlity analysis of Chapter III, and k,, k,, k,, k, are the 
vertical plane control gains as computed in Chapter III. The 
is for maximum 


existence of two distinct distances x xX 


dH / dv 


Flexibility in the design and to allow for the possibly 
Gifferent stability conditions for horizontal and vertical 
plane, as analyzed in the previous two chapters. 

Results are presented for a typical three dimensional 
commanded route that consists of the following way points (x, 
See = (20, 0, 5), (40, 5, 5), (60, -5, -3), (100, O, -5) 
vehicle lengths with individual straight line paths connecting 
them. Switch from one to the next straight line path was 
initiated when the vehicle position, measured along the 
current commanded path, was within a specified target distance 
(TD) from the way point. Parameters used for the simulation 
eee tollowing: t.=7, t =5, z.=0.1, €,=0.2, x,,=3, xX y,=2-5 
commanded speeds u=(4, 4, 5, 5) for the four straight line 
segments respectively, and TD=1l. Simulation results are 
presented in Figure 21 through 25. It can be seen from Figures 
21 that accurate path control is maintained in both the 
horizontal and vertical planes. Speed control is also very 
meeraLe, see Figure 22, despite the course changes and 
nonzero dive and rise angles. The speed controller revolutions 
per minute are shown in Figure 23, where the maximum 
saturation limit is set 500 rpm. Rudder response is shown in 


Figure 25 where the steady state nonzero values occur during 
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a nonzero commanded pitch angle. Comparing Figures 24 and 25” 
with 22, it can be observed that the vehicle slows down 
momentarily when the control surfaces become active, a 


Situation which is quickly corrected by the speed controller. 
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Figure 18. Coordinate transformation for 3-D path keeping 
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Figure 20. Vertical plane rotation 
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Figure 22. Time history of vehicle speed u 
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Figure 23. Time history of propeller revolutions per minute 
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Figure 24. Time history of rudder angle 
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Figure 25. Time history of dive plane angle 
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CONCLUSIONS AND RECOMMENDATIONS 

The main conclusions and contributions of this work can be 
summarized as follows: 

1. Pursuit guidance law and decoupled horizontal and 
vertical plane orientation controllers were shown to provide 
accurate vehicle path keeping in three dimensions. 

2. The scheme proved to be robust enough so that it could 
handle the nonlinear coupling between speed response, and 
horizontal and vertical plane motions without performance 
degradation. 

3. It was shown that the guidance and control schemes must 
be designed together in order to avoid loss of stability or 
excesSive oscillatory response. 

4. Analytic conditions for stability were derived. The 
conditions were expressed explicitly in terms of the vehicle 
hydrodynamic characteristics and the guidance and control law 
design specifications. 

5. An extensive study of the mechanism of loss of 
Stability was undertaken for the vertical plane motions. Two 
Gistinct possibilities were discovered and analyzed. In the 
first one pair of complex conjugate eigenvalues crosses the 
imaginary axis and results in an oscillatory vehicle behavior 
around the commanded path. In the second , one real eigenvalue 


crosses zero and the vehicle drifts off to a steady state path 


all 


with its deviation from the commanded path linearly increasing 
with time. This new path was computed and explicit conditions 


to avoid such a undesirable situation were given. 


Some recommendations for further research include the 
Ole wa ni < 

1. Comparisons from the point of view of path keeping 
response under physical system / mathematical model mismatch. 

2. State estimators must be included in the analysis to 
evaluate performance under partial state knowledge and sensor 


noise. 
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APPENDIX A 


PROGRAM SIM 3D.FOR 


FOTIS A. PAPOULIAS/ANGELLOS G. PAPASOTIRIOU 
NAVAL POSTGRADUATE SCHOOL 
AUGUST 1991 


VEHICLE THREE DIMENSIONAL PATH KEEPING 
HEADING AUTOPILOTS 

PURE PURSUIT NAVIGATION 

SIMULTANEOUS RUDDER/DIVE PLANE SWITCHINGS 


DECLARATIONS 


REEL MASS) IX, 1Y,L2,DXZ bya, EXY 

REAL K1H,K2H,K3H,K1V,K2V,K3V,K4V, KN 

REAL KPDOT, KRDOT,KPQ,KOR,KVDOT,KP,KR,KVQ,KWP, KWR,KV,KVWW, 
& KPN, KDB 

REAL MQDOT,MPP,MPR,MRR,MWDOT,MQO,MVP,MVR,MW,MVV, 

& MDS , MDB, NDRB 

REAL NPDOT,NRDOT,NPQ,NOR,NVDOT,NP,NR,NVO,NWP,NWR, 

& NV,NVW,NDRS 

REAL MM(6,6),INDX(100) 

DIMENSION X(9),BR(9),HH(9),VECH1(9),VECH2(9),XMMINV(6,6) 
DIMENSION VECV1(9),VECV2(9),F(12),FP(6),DISV(100) 
DIMENSION XDES(100),YDES(100),ZDES(100),UDES(100), 

& DISH(100) 


GEOMETRIC PROPERTIES 


WEIGHT=12000.0 
IX = 1700.0 
ey = 9450.0 
IZ =10700.0 
IL? se = 0.0 
TYZ = O70 
IXZ = Or 
L = P42 5 
RHO = 1.94 
G = O22 
XG = O70 
YG = 0.0 
AB = C0 
ae = O:...0 
ZB = 0.0 


TS 


Oaea 


aQaAND 


AO 
CDO 
MASS 
BOY 
RPMMAX= 
RPMMIN= 
UMAX 
UMIN 
ALPHA 


SURGE 


XPP 
XQOQ 
XRR 
XPR 
XUDOT 
XWO 
XVP 
XVR 
XODS 
XQDB 
XRDR 


: 


PS 

O 

~ 

O 

~ 
now Wl 


PS OX 
to 
7 GI 
Own 
i? 

| 


Dd) 
0.0057 


WEIGHT/G 
WEIGHT 


500.0 
500.0 
6.0 
ORE 


UMAX /RPMMAX 


7 


4. 
Te 


-O30E—037.08 
-1.470E-02*0. 
O1L0E-03*0. 
640E-04*0. 


-7.580E-03*0 


-3.240E-03*0 


Is 
De 


890E-02*0 
610E-02*0 


-2.600E-03*0 
-8.180E-04*0 


5 
it 


ile 
4. 
oe 


-/LOE=-01*0 
730E-03*0 
600E-02*0 
660E-03*0 


-1.160E-02*0 


-1.010E-02*0 


CDU-v 


HYDRODYNAMIC COEFFICIENTS 


> *KRHOF na 
5>*RAGs ieee 
> *RHO* ea 
>* KEHOE 


7 * RAO Aine ae 
~1.920E-01*0. 
-O* RO i> ae 
oi) EE © ees 
.>* ROS a 
~) * RHO sie 
.5* RHO L*ee 
290 202 70- 
27 RH O™ ie 
oO RHO ine 
2. * KAO] ba 
oo RO eee 
oo * RHO ieee 
-8.070E-03*0. 
oO RO eae a 
1 RRO bee 


> RUO* res 


> * RHO Eee 


5 *RAOF ie a 


XRES*ALPHA**2 


SWAY HYDRODYNAMIC COEFFICIENTS 


YEDor 
YRDOT 
YEO 
VOR 
YVDOT 


E 


NON NM WON & FF 


OV \O FF 


+ + 


NO NM 


»270E-04*0 
-240E-03*0 
~125E—-0320 
»5LOE-03*0 
-550E-02*0 
 UsoE—U02- 0 


. 360E-02*0 
»350E-01*0 
.880E-02*0 


. > * RHO ha 
. 2 * RHO sie <4 
5% RHO a4 
72> *RHOm baa 
70 RE @ wars 
-9 *RHOAbA=3 
-J/0K-02-0. 
#5* RAO ie 
oO “RHO Sia 
79 * REO ita 
S10 OZ Ue 
.840E-02*0. 
27 0H —O0Z08 
~-270E-02*0. 


5*RHO4 ba 


5 RHO tea 
5 * RG a 
5*RHO*L**2 
5 Rn @ eae 
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AAD 


QAM 


MaQ 


HEAVE HYDRODYNAMIC COEFFICIENTS 
ZQODOT =-6.810E-03*0.5*RHO*L**4 
SEE = 1.270E-04*0.5*RHO*L**4 
ZPR = 6.670E-03*0.5*RHO*L**4 
ZRR =-7.350E-03*0.5*RHO*L**4 
ZWDOT =-2.430E-01*0.5*RHO*L**3 
ZQ =-1.350E-01*0.5*RHO*L**3 
ZVP =—4 .810E-02*0.5*RHO*L**3 
ZVR =e. DOOR 02+ 00 5* RAO L* * 3 
ZW =——3-0Z0EF—-01*0. 5*RHO*L**2 
ZVV =-—6.840E-02*0.5*RHO*L**2 
ZDS ——2—a27 06—-02*0.5*RHO*L** 2 
ZDB =e) OF 0220. 5*RHO*L**2 


ROLL HYDRODYNAMIC COEFFICIENTS 


KPDOT =-1.010E-03*0.5*RHO*L**5 
KRDOT =-3.370E-05*0.5*RHO*L**5 
KPO =-6.930E-05*0 .5*RHO*L**5 
KOR = Soo OM a02 0. 5*RHO*L**5 
KVDOT = 1.270E-04*0.5*RHO*L**4 
KE =-1.100E-02*0.5*RHO*L**4 
KR =-8.410E-04*0.5*RHO*L**4 
KVQ =-5.115E-03*0.5*RHO*L**4 
KWP =-1.270E-04*0.5*RHO*L**4 
KWR Sees OR —O2~ 0, o~RHO*L** 4 
KV =) $3. 055H—03*0.5*RHO*L**3 
KVW Soy, OL OA G 5° REO*L* * 3 
PITCH HYDRODYNAMIC COEFFICIENTS 
MQDOT =-1.680E-02*0.5*RHO*L**5 
MPP eae —O5*O0.5*RHO*L**5 
MPR = 5.040E-03*0.5*RHO*L**5 
MRR =— 2 cOUK—03*0.5*RHO*L** 5 
MWDOT =-6.810E-02*0.5*RHO*L**4 
MQ =-6.860E-02*0.5*RHO*L**4 
MVP Sie —03*0.5*RHO*L* *4 
MVR = 1.730E-02*0.5*RHO*L**4 
MW = 9. 300E-02*0.5*RHO*L**3 
MVV =-2.510E-02*0.5*RHO*L**3 
MDS eet orO2*O.5*RHO*L**3 
MDB See St 0'2 *O>*REO*L=*s 


YAW HYDRODYNAMIC COEFFICIENTS 


NEDOTS——3.370B-05*0.5*RHO*L**5 
NRDOT =-3.400E-03*0.5*RHO*L**5 
NPQ ow OE—O2*0.5*REHO*L**5 
NOR —aee OE OS™ 0, 5*RHO-=L**5 


15 


QQea 


M6 


7@) 0) 


NVDOT 


NP 
NR 
NVQ 
NWP 
NWR 
NV 
NVW 
NDRS 
NDRB 


OPEN 


OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 
OPEN 


READ 
READ 


READ 
READ 


nun tt wou vu t tet 
~~] 


DATA 


»-240E-03%*0. 
-405E-04*0 
-640E-02*0 
so 20 -0s—0 
, OGRE -O02 “08 
. 390-0320. 
-420E-03%*0 
» 67 OR -02-0% 
211 3BS02 0° 
1 TSE-O02* C2 


> *RH@ran *4 
»5*RHO*L**4 
» SRC L* = 
.5* ROA 4 
5 “RH OR Tia 
D RRO ie 4 
.o* Rn ae 
5 REG Mies 
59 * RES 
D VINER rae 


AND RESULTS FILES 


(10,FILE='PATH 3D. DAT 7 soTalgc— emo 
(11,FILE="XY RES 5s 2aLus — New 
(12, FILE='XZ.RES' ,STATW@S= "NEW 
(13, FILE='DRS.RES' , STATUS='NEW' ) 
(14,FILE='DS.RES )STATUS— News 
(15, FILE='YCTE.RES@) SiR IVS =a ae 
(16, FILE='ZCTE.RES', STATUS='NEW' ) 
(17, FILE='XYZ.RES@™ STATUS=" NEW, 
(18,FILE='U.RES',STATUS='NEW' ) 
(19,FILE='RPM.RES',STATUS='NEW' ) 
(20, FILE='PHI.RES', STATUS='NEW' ) 
(21,FILE='THETA.RES' , STATUS='NEW' ) 
(22, FILE='PSI.RES’ , STATUS= 'NEW' ) 
(23, FILE='V.RES' , STATUS=' NEW") 
(24,PILE='R.RES (STRTUS—“ lew 
(25,FILE='W.RES' , STATUS='NEW' ) 
(26,FILE='Q.RES' ,STATUS='NEW' ) 
(27, FILE='YZ.RES' , STATUS='"NBW 


DATA 


FILE 


(10,*) TSIM, DELTA, IPRNT 
(10,*) IPTS, TARGET 
(10,*) TN,TH,TV,ZG 
IF (IPTS.GT.100) IPTS=100 
DO 1 I=1,IPTS 
READ (10,*) XD,YD,ZD,XDH,XDV,UO 
XDES(1)=XD*L 
YDES(1I)=YD*L 
ZDES(I)=ZD*L 
UDES(I)=U0 
DISH(I)=XDH*L 
DISV(I)=XDV*L 
CONTINUE 


MASS MATRIX INITIALIZATION AND DEFINITION 


DO 15 J=1,6 
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MO 


(0) © 


10 
eS 


a1 


12 


3 


DO 10 K=1,6 
XMMINV(J,K)=0.0 
MM(J,K)=0.0 

CONTINUE 

CONTINUE 


MM(1,1)= MASS-XUDOT 
MM(1,5)= MASS*ZG 
MM(1,6)=-MASS*YG 


MM(2,2)= MASS-YVDOT 
MM(2,4)=-MASS*ZG-YPDOT 
MM(2,6)= MASS*XG-YRDOT 


MM(3,3)= MASS-ZWDOT 
MM(3,4)= MASS*YG 
MM(3,5)=-MASS*XG-ZQDOT 


MM(4,2)=-MASS*ZG-KVDOT 
MM(4,3)= MASS*YG 
MM(4,4)= IX-KPDOT 
MM(4,5)=-IXY 
MM(4,6)=-IXZ-KRDOT 


MM(5,1)= MASS*ZG 
MM(5,3)=-MASS*XG-MWDOT 
MM(5,4)=-IXY 

MM(5,5)= IY-MQDOT 
MM(5,6)=-IYZ 


MM(6,1)=-MASS*YG 
MM(6,2)= MASS*XG-NVDOT 
MM(6,4)=-IXZ-NPDOT 
MM(6,5)=-IYZ 

MM(6,6)= IZ-NRDOT 


MASS MATRIX INVERSION 


DO 12 1=1,6 

DO 11 J=1,6 

XMMINV(I,J)=0.0 

CONTINUE 

XMMINV(I,1I)=1.0 
CONTINUE 
CALL INVTA(MM, 6, INDX,D) 
DO 13 J=1,6 

CALL INVTB(MM,6, INDX,XMMINV(1,J)) 
CONTINUE 


VARIABLES INITIALIZATION 


a 


PISIM =TSIM/DELTA 


AES 


ISIM =PISIM 
ECHO ~=)707/ DELTA 
IECHO =ECHO 

YAW =0.0 

SWAY =0.0 

Pitch) Oe 

HEAVE =0.0 

U =JDESt ) 
RPM =UDES(1)/ALPHA 
V =0.0 

W =6 0 

P =e 

Q =O 

R =0.0 

DS =0.0 

DB = 0 

DR = 01.0 

TWOPI =8.0*ATAN(1.0) 
PI =0.5*TWOPI 
PHL) =O 
ISTART=1 
TARGET=TARGET*L 
XPOS =0.0 

YPOS =0.0 

ZPOS =0.0 

CD Yan —oeS 

ED 

JPRNT =0 
iO 

JE = 

DRS =0.0 

DRB =0.0 

DS =) 0 

DB =) ,0 


DEFINE THE LENGTH X, BREADTH BR, AND HEICHT Hie TERMS 


X(1) = 921059712 .6 
X(2) = =99537i7me 
X(2)0 = | e731 
X(4) = -66.3/12.0 
X(5) = 727 eo 
X(6) = 2202 lO 
X(7) = Oa 27 0 
X(8) = 99.2/12.0 
X(9) = 103.2/12.0 
HH(1) = O00 1240 
HH(2) = 8.24/12.0 
HH(3) = 19.76/12.0 
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CQO 


AANA 


HH(4) = 29.36/12.0 
HH(5) = 31.85/12.0 
HH(6) = 27.84/12.0 
HH(7) = 21.44/12.0 
HH(8) = 12.00/12.0 
HH(9) = 00sec 
BR(1) = O00 12.0 
BR(2) = Bae 4/1250 
BR(3) = 19.76/12.0 
BR(4) = 29.36/12.0 
BR(5) = 31.85/12.0 
ERVGhH ee 27 2647 12150 
BR(7) =  21.44/12.0 
BR(8) = 12.00/12.0 
BR(9) = 0.00/12.0 


AUXILLIARY VARIABLES FOR HORIZONTAL PLANE CONTROL 


DH =(IZ-NRDOT)*(MASS-YVDOT) - 

& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
A11H=((1Z-NRDOT) *YV-(MASS*XG-YRDOT) *NV) /DH 
A12H=((IZ-NRDOT) *(-MASS+YR)- 

& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
A21H=((MASS-YVDOT) *NV-(MASS*XG-NVDOT) *YV) /DH 
A22H=( (MASS-YVDOT) * (-MASS*XG+NR) - 

& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
B11H=((1IZ-NRDOT) *YDRS-(MASS*XG-YRDOT) *NDRS) /DH 
B12H=((1IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
B21H=((MASS-YVDOT ) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
B22H=((MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 
B1H =B11H-B12H 
B2H =B21H-B22H 


AUXILLIARY VARIABLES FOR VERTICAL PLANE CONTROL 


DV =(MASS-ZWDOT) * (IY-MQDOT ) -ZQDOT*MWDOT 
A11V=(( IY-MQDOT) *ZW+ZQDOT*MW)/DV 
A1l2V=((IY-MQDOT) *(ZQ+MASS )+ZQDOT*MQ) /DV 
A13V=-(ZG-ZB) * (MASS*XG+ZQDOT ) *WEIGHT/DV 
A21V=(MWDOT*ZW+(MASS-ZWDOT ) *MW) /DV 
A22V=(MWDOT* (ZQ+MASS )+(MASS-ZWDOT ) *MQ) /DV 
A23V=-(ZG-ZB) * (MASS-ZWDOT ) *WEIGHT/DV 
B11V=( ( 1Y-MQDOT ) *ZDS+ZQDOT*MDS ) /DV 
B12V=((IY-MQDOT ) *ZDB+ZQDOT*MDB) /DV 
B21V=(MWDOT* ZDS+(MASS-ZWDOT ) *MDS) /DV 
B22V=(MWDOT*ZDB+(MASS-ZWDOT ) *MDB) /DV 

B1V =B11V-B12V 

B2V =B21V-B22V 


US 


OO Ge 


OOra 


210 


21 


SIMULATION BEGINS 
LOOP OVER WAY POINTS 


DO 200 Vee ies 


IF (IP.GE.2) CO Tome 
XDH=DISH(1) 
XDV=DISV(1) 
U0 =UpHs( 1) 
XD =<pasrl 
YD =YDES(1) 
ZD =ZDES(1) 
XD1=0-0 
YDI=0.0 
7ZD1=0.0 
XD2=XD 
YD2=YD 
ZD2=ZD 

GO TOs 
XDH=DISH(IP) 
XDV=DISV(IP) 
UO =UDES(IP) 
XD =XDEGGmE) 
YD =YDES(IP) 
7 De ADEs, 2) 
XD1=XD2 
YD1=YD2 
ZD1=ZD2 
XD2=XD 
YD2=YD 
7ZD2=2D 
7D12=7D2-2D1 
XDiZ=<De onl 
pen 


HORIZONTAL HEADING CONTROL GAINS 


OMEGAH=(10.0*U0)/(TH*L) 
AD1H=1.75*OMEGAH 
AD2H=2.15*OMEGAH**2 

AD3H=OMEGAH* * 3 

A1=B1H*U0*UO0 

B1=B2H*U0*UO 
C1=-AD1H-(A11H+A22H) *UO 
A2=(B1H*A22H-B2H*A12H) *U0**3 
B2=(B2H*A11H-B1H*A21H) *U0**3 
K1H=AD3H/((B2H*A11H-B1H*A21H) *U0**3) 
C2=AD2H-(A11H*A22H-A12H*A21H) *U0**2+B2H*U0*U0*KI1H 
K2H=(C1*B2-C2*B1)/(A1*B2-A2*B1) 
K3H=(C2*A1-C1*A2) /(Al1*B2-A2*B1) 
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GEG @) 


VERTICAL HEADING CONTROL GAINS 


OMEGAV=(10.0*U0)/(TV*L) 
AD1V=1.75*OMEGAV 
AD2V=2.15*OMEGAV* *2 

AD3V=OMEGAV* *3 

A2=B1V*U0*U0 

A3=B2V*U0*U0 

D1=-AD1V-(A11V+A22V) *U0 
B1=-B2V*U0*U0 
B2=(B1V*A22V-B2V*A12V) *U0**3 
B3=(B2V*A11V-B1V*A21V) *U0**3 
D2=AD2V+A23V+(A12V*A21V-A11V*A22V) *U0**2 
C1l=(B2V*A11V-B1V*A21V) *U0**3 
C2=(A23V*B1V-A13V*B2V) *U0**2 
D3=AD3V+(A13V*A21V-A11V*A23V) *U0 
K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 
K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2 ) 
K1V=(D3-C2*K2V) /Cl 
K3V=(D1-A2*K2V) /A3 


ALPHAH=ATAN(YD12/XD12) 

ALPHAH=ABS (ALPHAH) 

IF ((XD12.GE.0.0).AND.(YD12.GE.0.0)) ALPHAH= ALPHAH 
IF ((XD12.GE.0.0).AND.(YD12.LT.0.0)) ALPHAH= -ALPHAH 
IF ((XD12.LT.0.0).AND. (YD12.GE.0.0)) ALPHAH=PI-ALPHAH 
IF ((XD12.LT.0.0).AND. (YD12.LT.0.0)) ALPHAH=PI+ALPHAH 
XCTEH=(YPOS-YD1) *SIN(ALPHAH )+(XPOS-XD1 ) *COS (ALPHAH) 
YCTE =(YPOS-YD1) *COS(ALPHAH ) -(XPOS-XD1) *SIN(ALPHAH) 
X1P =YD12*SIN(ALPHAH)+XD12*COS(ALPHAH) 
ALPHAV=ATAN(ZD12/X1P) 

ALPHAV=ABS (ALPHAV ) 

IF (ZD12.GE.0.0) ALPHAV=-ALPHAV 

K4V=- (A13V* (A21V+B2V*U0*K2V ) -A23V* (A11V+B1V*U0*K2V) ) 
K4V=K4V*SIN(ALPHAV) /( (B1V*A21V-B2V*A11V) *U0*UO) 

ZCTE = (ZPOS-ZD1)*COS(ALPHAV) +XCTEH*SIN(ALPHAV) 
XCTEV=-(ZPOS-ZD1) *SIN(ALPHAV ) +XCTEH*COS (ALPHAV) 


PROPULSION CONTROL GAIN 


WSS=(B1V*A23V-B2V*A13V) *SIN(ALPHAV) 
WSS=WSS/((A11V*B2V-A21V*B1V) *UO) 
DSS=(A21V*A13V-Al11V*A23V) *SIN(ALPHAV) 
DSS=DSS/((Al11V*B2V-A21V*B1V) *U0*UO) 
FUC=XWW*WSS* *2+U0*WSS* (XWDS-XWDB) *DSS 

+U0*U0* (XDSDS+XDBDB ) *DSS* *2-XRES*U0**2 
RPMO=-FUC/ (XRES*ALPHA**2) 
RPMO=SORT(RPMO) 
WRITE (*,*) RPMO,UO/ALPHA 
KN=-5.0*U0* (MASS-XUDOT) / (XRES *ALPHA*ALPHA* RPMO*TN*L) 
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© C2 Oe 


QO 


COA OG 


600 


601 


602 


OM 2 —| — — — —K 


WRITE (*,201) XD/L,YD/L,2D/L 
SIMULATION FOR EACH WAY POINT 


DO 10OS=TSTART As 
ICOUNT=I1 


IF (U.LT.UMIN) U=UMIN 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER 
THE Yee tene 


DO 600 K=1,9 
UCF=(V+X(K) *R) **2+(W-X(K) *Q) **2 
UCF=SORT(UCF) 

IF (UCF.LT.1.E-6) Conte soe 

CFLOW=CDY*HH(K) *(V+X(K) *R) **2+CDZ*BR(K) * 
(W-X(K)*Q) **2 

VECH1(K) =CFLOW* (V+X(K)*R) /UCF 

VECH2 (K) =CFLOW* (V+X(K)*R)*X(K) /UCF 

VECV1(K)=CFLOW* (W-X(K)*Q) /UCF 

VECV2 (K) =CFLOW* (W-X(K) *Q) *X(K) /UCF 

CONTINUE 

CALL TRAP(9,VECV1,X,HEAVE) 

CALL TRAP(9,VECV2,X, PITCH) 

CALL TRAP(9,VECH1,X,SWAY ) 

CALL TRAP(9,VECH2,X,YAW  ) 

HEAVE=-0.5*RHO*HEAVE 

PITCH=+0.5*RHO*PITCH 

SWAY =-0.5*RHO*SWAY 

YAW =-0.5*RHO*YAW 

GO TO 602 

HEAVE=0.0 

PITCH=0.0 

SWAY =0.0 

YAW =0.0 

CONTINUE 


FORCE EQUATIONS 


SURGE FORCE 


FP(1) = MASS*V*R-MASS*W*Q+MASS*XG*Q* *2+MASS*XG*R* *2- 
MASS* YG*P*Q-MASS*ZG*P*R+XPP*P**2+XQQ* 
QO**2+XRR*R**2+XPR*P*R+XWO*W*O+XVP*V* P+ 
XVR*V*R+U*Q* (XODS*DS+XQDB*DB) + 
U*R* (XRDRS*DRS+XRDRB*DRB) +XVV*V* *2+XWW* 

W* *2+U*V* (XVDRS*DRS+XDRB*DRB) +U*W* 
(XWDS*DS+XWDB*DB) +(XDSDS*DS**2+XDBDB*DB* * 2+ 
XDRDR* (DRS**2+DRB**2) )*U**2-(WEIGHT-BOY) * 
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Sk © ee 


Cry ©) 


aqQacQY 


Crane) 


ye) ©) 


RQ? 2 Qo By —K QR? Qo QM LM 


RM —] — — QR RK 


RM — 2] WS LI? Qo Lr Q 


M 2 —9 — LM —K 


SIN( THETA ) +XPROP* RPM*RPM-XRES*U*U 


SWAY FORCE 


FP(2) =-MASS*U*R-MASS*XG*P*Q+MASS* YG*R* *2-MASS*ZG*Q*R+ 


YPQ*P*Q+YQR*Q*R+YP*U*P+YR*U*R+YVO*V*OF 

YWP*W* P+ YWR*W*R+YV*U* V+YVW* V*W+YDRS*U**2*DRS+ 
YDRB*U* *2*DRB+ (WEIGHT-BOY) * 

COS( THETA) *SIN(PHI )+MASS*W*P+MASS* YG* P* *2+SWAY 


HEAVE FORCE 


FP(3) = MASS*U*Q-MASS*V*P-MASS*XG*P*R-MASS*YG*Q*R+ 
MASS*ZG*P* *2+MASS*ZG*Q* *2+ZPP*P**24 
ZPR*P*R+ZRR*R**24+Z0% 
U*Q+ZVP*V*P+ZVR*V*R+ZW* U*W+ZVV*V* *2+HEAVE+ 
U**2* (ZDS*DS+ZDB*DB) +(WEIGHT-BOY ) * 

COS (THETA) *COS( PHI) 


ROLL MOMENT 


FP(4) = -IZ*Q*R+IY*Q*R-IXY*P*R+1IYZ*Q**2- 
IYZ*R**2+1XZ*P*Q+MASS*YG*U*Q-MASS* 
YG*V*P-MASS* ZG*W* P+KPQ* P*Q+KOR*Q*R+ 
KP *U*P+KR*U*R+KVO*V*O+KWP* W* P+ 
KWR*W*R+KV*U*V+KVW*V*W+ ( YG*WE IGHT-YB*BOY ) * 
COS (THETA) *COS (PHI) -(ZG*WEIGHT- 

ZB*BOY ) *COS (THETA) *SIN(PHI)+MASS*ZG*U*R 


PITCH MOMENT 


FP(5) = -IX*P*R+IZ*P*R+IXY*Q*R-IYZ*P*Q- 
IXZ*P**2+1XZ*R* *2-MASS*XG*U*Q+ 
MASS*XG*V* P+MASS*2G*V*R- 
MASS*ZG*W*Q+MPP*P**2+ 
MPR*P*R+MRR*R**2+MQ* 
U*Q+MVP*V*P+MVR*V*R+MW* U* W+ 
MVV*V**24+U* *2* (MDS*DS+MDB*DB ) - (XG*WEIGHT- 
XB* BOY) *COS (THETA) *COS(PHI)- 
(ZG*WEIGHT-ZB*BOY ) *SIN( THETA)+P ITCH 


YAW MOMENT 


FP(6) = -IY*P*Q+IX*P*Q+IXY*P**2-IXY*Q**2+1IYZ*P*R- 
IXZ*Q*R-MASS*XG*U*R+MASS*XG*W*P-MASS*YG* 
V*R+MASS* YG*W*QO+NPO*P*Q+NOR*Q*R+NP*U* P+NR* 
U*R+NVO*V*Q+NWP *W*P+NWR*W*R+NV*U* V+ 
NVW*V*W+NDRS*U* * 2* DRS+NDRB*U**2*DRB+ 
(XG*WEIGHT-XB* BOY ) *COS (THETA) *SIN(PHI)+ 
(YG*WEIGHT-YB*BOY ) *SIN( THETA) +YAW 


83 


CY Qe 


C2 O)s@ 


re) 


yO @ 


OO @ 


Ol 
610 


COMPUTE THE RIGHT HAND SIDE OF XDOT=F(X) 


DO 610 J 


B(J) 


DO 611 
F(J) 


ao 


lor 


f 


1,6 
XMMINV(J,K)*FP(K) + F(J) 


IL aS 


CONTINUE 
CONTINUE 


INERTIAL POSITION ara 


F(7) 


F(8) 


F(9) = 


U*COS (PSI) *COS (THETA) +V* (COS(PSI)*SIN( THETA) * 


SIN(PHI)-SIN(PSI) *COS(PHI) )+W*(COS(PSI)* 
SIN( THETA) *COS(PHI)+SIN(PSI)*SIN(PHI)) 


U*SIN(PSI)*COS(THETA)+V*(SIN(PSI)*SIN( THETA) * 


SIN(PHI)+COS(PSI)*COS(PHI) )+W*(SIN(PSI)* 
SIN( THETA) *COS(PHI)-COS(PSI)*SIN(PHI)) 


-U*SIN( THETA) +V*COS (THETA) *SIN(PHI)+ 
W*COS (THETA) *COS(PHI) 


EULER ANGLE RATES 


F(10)= P+Q*SIN(PHI)*TAN(THETA)+R*COS (PHI) *TAN( THETA) 


F(11)= 
F(12)= 
ASSIGN 


UDOT 
VDOT 
WDOT 
PpOT 
QDOT 
ROT 
XDOT 
YDor 
ZDOT 
Ene 
THEDOT 
Psiber 


FoR Sus 
ID 


V 
W 


Q*COS(PHI)-R*SIN(PHI) 
O*SIN(PHI ) /COS( THETA) +R*COS (PHI) /COS(THETA) 
XDOT VECTOR 


F(1) 
(2) 
F(3) 
B (4) 
F(5) 
F(6) 
F(7) 
F(8) 
F(9) 
F(10) 
(a 
Bidz 


ORDER INTEGRATION 


U + DELTA-Uber 
V + DELTA*V oer 
W + DELTA*WDOT 
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COO OrGre) Cr Gro) 


One © 


©_Or@ 


E = P a el ea 
0 = Q i DE LTA~ODOr 
R = R wen hia Ree. 
XPOS = XPOS + DELTA*XDOT 
weGS) = YEOsS. + DELTA*YDOL 
ZPOS = ZPOS + DELTA*ZDOT 
eel = PHI + DELTAS PAIDOT 
Tope = habla] +) DELTA*THEDOT 
Pol = PSI + DELTA -PSsiDor 


VELOCITY INPUT CALCULATION 


UC=U0 
IF (UC.GE.UMAX) UC=UMAX 
IF (UC.LE.UMIN) UC=UMIN 


RPM INPUT CALCULATION 


RPMO=UC/ALPHA 

RPM=RPM0+KN* (U-UC) 

IF (RPM.GE.RPMMAX) RPM=RPMMAX 
IF (RPM.LE.RPMMIN) RPM=RPMMIN 


COORDINATE TRANSFORMATIONS 


XCTEH= (YPOS-YD1)*SIN(ALPHAH)+(XPOS-XD1) *COS(ALPHAH) 
YCTE = (YPOS-YD1)*COS(ALPHAH ) -(XPOS-XD1) *SIN(ALPHAH) 
ZCTE = (ZPOS-ZD1) *COS(ALPHAV) +XCTEH*SIN(ALPHAV) 
XCTEV=- (ZPOS-ZD1) *SIN(ALPHAV) +XCTEH*COS (ALPHAV) 


Hee vCR lek TA 


VTOTAL= (XD2-XD1)**2+(ZD2-ZD1) **2 

VTOTAL=SORT(VTOTAL ) 

HTOTAL=(XD2-XD1)**2+(YD2-YD1)**2 

HTOTAL=SORT(HTOTAL) 

VAWAY =VTOTAL-XCTEV 

VAWAY =ABS(VAWAY) 

HAWAY =HTOTAL-XCTEH 

HAWAY =ABS(HAWAY) 

IF ((VAWAY.LT. TARGET) .OR.(HAWAY.LT.TARGET)) GO TO 
101 


DIVE PLANE INPUT CALCULATION 

ZPHI=ZCTE 

SIGV=ATAN(ZPHI/XDV) 

DS=K1V* (THETA-ALPHAV-SIGV) +K2V*W+K3V*Q+K4V 
[mIDSNGE. 0.4) DS= 0.4 

iim Doekee-0.4) DS=-0.4 
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C) CQ 


Oa 


ae 


100 


Lou 
200 
500 
INS) 


& 


DB=-DS 


RUDDER INPUT CALCULATION 


YPHI=YCTE 
SIGH=-ATAN (YPHI/XDH) 


DRS=K1H* (PSI-ALPHAH-SIGH )+K2H*V+K3H*R 


IF (DRS .GEP 


DRB=-DRS 


PRINT RESUETS 


TIME=I*DELTA 
JE=JE+1 


IF (JE.NE.IECHO) GO TO 99 


JE=0 


JPRNT=JPRNT+1 


IF (JPRNT.NE.IPRNT) GO TO 100 


IJK=IJK+1 

TIME=I*DELTA 
(11,*) XPOS/L, Yeos, 
(2), * ) XPOS/L,ZEes Lu 
(13,*) TIME,DRS*180.0/PI 


Wiis 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
Eee s 
WRITE 


(14,*) TIME,DS*180.0/PI 
(15,*) TIME, YCTE/L 
(16,*)) Dive eae tay i 


0.4) DRS= 0.4 
IF (DRS.LE.-0.4) DRS=-0.4 


(17,*) XPOS/L, YPOS/L, Zeosy = 


(18,*) TIME,U 


(19,*) TIME,RPM 


(20,*) TIME, PHI*180.0/PI 
(21,*) TIME, (THETA-ALPHAV)*180.0/PI 
(22,*) TIME, (PSI-ALPHAH) *180.0/PI 


(23,*) TIME,V 
(24,*) TIME,R 
(25,*) TIME,W 
(26,*) TIME,Q 


(27,*) YPOS/l, zZeesyu 
IPRNT=0 


CONTINUE 

GO TO 500 

ISTART=ICOUNT 
CONTINUE 


SOE 


FORMAT (' 


END 


' 
/ 


HEADING FOR (X,Y,Z) 


FO.3° O88 
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( 


| Fone: 


/ 


oon 


/ 


Qa 


ra 


2 


MN 


14 


5 


tec cs ce cc ce cc ce cc ce ce ce ccc cc mmc cc cm cc ee ee ee 
ec ccc cc crm crc cr crs cc ce cece ccc cmc cr cmc crm cr ccm cc cc ce cee ee ee 


SUBROUTINE TRAP(N,A,B,OUT) 


NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 


DIMENSION A(1),B(1) 

N1=N-1 

OUT=0.0 

Dome — ihe 
Oumil—Os54(A( lytA I+l))*(BCI+1)-B¢1)) 
OUT =OUT+OUT1 

CONTINUE 

RETURN 

END 


em cmc mm cr me wr rrr eee ee eee eee 
ecm r mecmmcm mrmcr ccmcrccrccrcmcre 


SUBROUTINE INVTA(MM,N, INDX,D) 


PARAMETER (NMAX=100,TINY=1.0E-20) 
DIMENSION INDX(6),VV(NMAX) 
REAL MM(6,6) 
D=1 
DO 12 I=1,N 
AAMAX=0. 
DO 11 J=1,N 
IF(ABS(MM(I,J)).GT.AAMAX) AAMAX=ABS(MM(I,J)) 
CONTINUE 
IF (AAMAX.EQ.0.) PAUSE 'SINGULAR MATRIX’ 
VV(1)=1./AAMAX 
CONTINUE 
DO 19 J=1,N 
DOM t—1 g-1 
SUM=MM(I,J) 
DO 13 K=1,I-1 
SUM=SUM-MM(1,K)*MM(K,d) 
CONTINUE 
MM(I,J)=SUM 
CONTINUE 
AAMAX=0. 
DO 16 I=J,N 
SUM=MM(I,J) 
DO 15 k=1,J-1 
SUM=SUM-MM(I,K)*MM(K,J) 
CONTINUE 
MM(I,J)=SUM 
DUM=VV (1) *ABS(SUM) 
IF (DUM.GE.AAMAX) THEN 
IMAX=1 
AAMAX=DUM 
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eG 


Ih 


18 


ee 


12 


IS, 


14 


ENDIF 
CONTINUE 
TE (CI. NE UMA Eien 
DO 17 K=1,N 
DUM=MM( IMAX,K) 
MM( IMAX, K)=MM(J,K) 
MM(J,K)=DUM 
CONTINUE 
D=-D 
VV ( IMAX)=VV(J) 
ENDIF 
INDX(J)=IMAX 
IF (MM(G70). 50.0. MMC 3) Fy 
TF(J NE. Nn 
DUM=1./MM(J,J) 
DO” 18: 2t=a4 aN 
MM(I,J)=MM(1I,J)*DUM 
CONTINUE 
ENDIF 
CONTINUE 
RETURN 
END 


ee a ES 
em cmc cm crm crc mc ccc cmc crm crm crm cr rm cme crm rec crrmrmc c cs es e e e 


SUBROUTINE INVTB(MM,N, INDX,B) 
DIMENSION INDX(N),B(N) 
REAL MM(6,6) 
TI=0. 
DO 12 I=1,N 
LL=INDX(TI) 
SUM=B(LL) 
B(LL)=B(1I) 
IF (II.NE.0)THEN 
DO 11 g=11 1-8 
SUM=SUM-MM(I,J)*B(J) 
CONTINUE 
ELSE IF (SUM.NE.0) THEN 
II=1 
ENDIF 
B(1I)=SUM 
CONTINUE 
DO 14 I=N,1,-1 
SUM=B (TI) 
IF (I.LT.N) THEN 
DO 13 J=I+1,N 
SUM=SUM-MM(I,J)*B(J) 
CONTINUE 
ENDIF 
B(I)=SUM/MM(I,I) 
CONTINUE 
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ee) 





100 OF Oe 


APPENDIX B 


PROGRAM VERT STAB.FOR 


REGIONS OF STABILITY ~ VERTICAL PLANE 

PARAMETERS ARE: XD AND TV 

NUMERICAL OR ANALYTIC COMPUTATION 

IT NEEDS FILE "SUBRTNS.FOR" OR ANY STANDARD EIGENVALUE 
SOLVER 


IMPLICIT DOUBLE PRECISION (A-H,0O-Z) 

DOUBLE PRECISION K1V,K2V,K3V,L 

DOUBLE PRECISION MOQDOT,MO,MW,MWDOT,MDS,MDB,MASS,IY 
DIMENSION A(4,4),FV1(4),1V1(4),222(4,4),WR(4),WI(4) 


OPEN (10,FILE='BIFO.RES |{STaAnlus— Ves 


OPEN (11,FILE='BIF1.RES',STATUS='NEW' ) 
OPEN (12,FILE='BIF2.RES',STATUS='NEW' ) 
OPEN (13,FILE='BIF3.RES',STATUS='NEW' ) 


WE IGHT=12000.0 


ny = 9450.0 

1 = 17.425 

RHO = 1.94 

G = 22 

se : G0 

ZB = 0.0 

MASS =WEIGHT/G 

BOY =WEIGHT 

ZQDOT =-6.810E-03*0.5*RHO*L**4 
ZWDOT =-2.430E-01*0.5*RHO*L**3 
Z0 =-1.350E-01*0.5*RHO*L**3 
ZW =-3.020E-01*0.5*RHO*L**2 
ZDS =-2.270E-02*0.5*RHO*L**2 
ZDB =-2.270E-02*0.5*RHO*L**2 
MODOT =-1.680E-02*0.5*RHO*L**5 
MWDOT =-6.810E-02*0.5*RHO*L**4 
MOQ =-6.860E-02*0.5*RHO*L**4 
MW = 9.860E-02*0.5*RHO*L**3 
MDS =-1.113E-02*0.5*RHO*L**3 
MDB = 1.113E-02*0.5*RHO*L**3 
WRITE (*,1001) 

READ (*,*) TVMIN, TVMAX, ITV 


WRITE (*,1002) 


READ 


a) 


XDMIN, XDMAX, IXD 
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Crore 


QaN 


XDMIN=XDMIN*L 
XDMAX=XDMAX*L 
WRITE (*,1003) 


READ (*,*) U,ZG 
WRITE (*,1004) 
READ (*,*) ISOL 


AUXILIARY VARIABLES 


DV =(MASS-ZWDOT) * (IY-MQDOT)-ZQDOT*MWDOT 
Al1V=((IY-MQDOT ) *ZW+ZQDOT*MW) /DV 
Al12V=((IY-MQDOT) *(ZQ+MASS)+ZQDOT*MQ) /DV 
A13V=-(ZG-ZB) * (MASS*XG+ZQDOT ) *WEIGHT/DV 
A21V=(MWDOT* ZW+(MASS-ZWDOT) *MW) /DV 
A22V=(MWDOT* (ZQ+MASS )+(MASS-ZWDOT) *MQ) /DV 
A23V=-(ZG-ZB) * (MASS-ZWDOT ) *WEIGHT/DV 
B11V=((IY-MQDOT) *ZDS+ZQDOT*MDS) /DV 
B12V=(( IY-MODOT) *ZDB+ZQDOT*MDB) /DV 
B21V=(MWDOT*ZDS+(MASS-ZWDOT) *MDS) /DV 
B22V=(MWDOT*ZDB+ (MASS-ZWDOT) *MDB) /DV 

B1V =B11V-B12V 

B2V =Be1V=B22V 


EPS =1.D-5 
ILMAX=1500 


LOOP OVER TV 


DO 1 I=1,ITV 
WRITE (*,2001) I,ITV 
TV=TVMIN+(I-1)*(TVMAX-TVMIN) /(ITV-1) 
OMEGAV=(10.0*U)/(TV*L) 
AD1V=1.75*OMEGAV 
AD2V=2.15*OMEGAV**2 
AD3V=OMEGAV* * 3 
A2=B1V*U*U 
A3=B2V*U*U 
D1=-AD1V-(A11V+A22V) *U 
B1=-B2V*U*U 
B2=(B1V*A22V-B2V*A12V) *U**3 
B3=(B2V*A11V-B1V*A21V) *U**3 
D2=AD2V+A23V+ (A12V*A21V-A11V*A22V) *U**2 
C1=(B2V*A11V-B1V*A21V) *U**3 
C2=(A23V*B1V-A13V*B2V) *U**2 
D3=AD3V+(A13V*A21V-A11V*A23V) *U 
K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 
K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 
K1V=(D3-C2*K2V) /Cl 
K3V=(D1-A2*K2V) /A3 
D333=(A13V*A21V-A11V*A23V) *U 
XAAA=CBRT(-D333) 


Gall 


QQ) @ae 


MQQMQ. 0 


Ze 


D334=(D3*B2*C1*A3+B3*C1*A2*A3—-B3*Ci* Wi G2 — ie 
D335=B3*C1*A2+B2*Cl*As= eee 
D336=D334/D335 
XBBB=CBRT (D336) 


ANALYTICAL COMPUTATION 


IF (ZG.NE.0.0) TVCR1=(10.*U)/(XAAA*L) 
IF (2ZG.NE.0.0) TVCR2=(10.*U)/(XAAA*L) 
IF (ISOLVEOVONMEO TOM 
CXD2=AD3V* (AD1V*AD2V-AD3V) 
CXD1=~(B2+A3*U) *K1V* (AD1V*AD2V-AD3V) +AD3V*K1V* 
(A2*AD1V+B2+A3*U) -AD1V*AD1V*K1V* (-C2+C1*U) 

CXDO=- (B2+A3*U) *K1V*K1V* (AD1V*A2+B2+A3*U) 
DET=CXD1*€xD1_490*CxD2 ene 
IF (DET.LT.0. OjmComre ml 
XD1=(-CXD1+DSQORT(DET) )/(2.0*CXD2) 
XD2=(-CXD1-DSQRT( DET) )/(2.0*CXD2) 
IF (XD1.NE.0.0) 

VAL1=AD3V+( (B2V*A12V-B1V*A22V-B2V) *K1V*U**3) /XD1 
IF (XD2. NEG s0) 

VAL2=AD3V+( (B2V*A12V-B1V*A22V-B2V) *K1V*U**3) /XD2 
GO TO 23 


NUMERICAL COMPUTATION 
LOOP OVER XD 


DO 2 J=1,I1XD 
XD=XDMIN+(J-1)*(XDMAX-XDMIN) /(IXD-1) 
THETA=0.0D0 
CT=DCOS (THETA) 

ST=DSIN( THETA) 

W=0.0D0 

AG Orme DO 

A(1,2)=0.0D0 

A(1,3)=1.0D0 

A(1,4)=0.0D0 
A(2,1)=B1V*U*U*K1V+A13V*CT 
A(2,2)=B1V*U*U*K2V+A11V*U 
A(2,3)=B1V*U*U*K3V+A12V*U 
A(2,4)=-B1V*U*U*K1V/XD 
A(3,1)=B2V*U*U*K1V+A23V*CT 
A(3,2)=B2V*U*U*K2V+A21V*U 
A(3,3)=B2V*U*U*K3V+A22V*U 
A(3,4)=-B2V*U*U*K1V/XD 
A(4,1)=-U*€E=wesm 
A(4,2)=CT 

A(4)3) 6 robo 

A(4,4)=0.0D0 


COMPUTE EIGENVALUES 


a2 


10 


CALL RG(4,4,A,WR,WI1,0,22Z,1V1,FV1,1ERR) 
CALL DSTABL(DEOS,WR,WI,FREQ) 


hd .CT, 1) COeTO 10 
DEOSOO=DEOS 


XDOO =XD 
LL=0 

GO TO 2 
DEOSNN=DEOS 
ADNN =XD 


PR=DEOSNN*DEOSOO 

iu PReGr.0.DO) CO TO 3 
LL=LL+1 

IF (LL.GT.3) STOP 1000 
IL=0 

XDO=XDOO 

XDN=XDNN 

DEOSO=DEOSOO 

DEOSN=DEOSNN 

XDL=XDO 

XDR=XDN 

DEOSL=DEOSO 

DEOSR=DEOSN 

XD=(XDL+XDR) /2.D0 
A(1,1)=0.0D0 

A(1,2)=0.0D0 

A(1,3)=1.0D0 

A(1,4)=0.0D0 
A(2,1)=B1V*U*U*K1V+A13V*CT 
A(2,2)=B1V*U*U*K2V+A11V*U 
A(2,3)=B1V*U*U*K3V+A12V*U 
A(2,4)=-B1V*U*U*K1V/XD 
A(3,1)=B2V*U*U*K1V+A23V*CT 
A(3,2)=B2V*U*U*K2V+A21V*U 
A(3,3)=B2V*U*U*K3V+A22V*U 
A(3,4)=-B2V*U*U*K1V/XD 
A(4,1)=-U*CT-W*ST 
A(4,2)=CT 

A(4,3)=0.0D0 

A(4,4)=0.0D0 


CALL RG(4,4,A,WR,W1,0,22Z,1V1,FV1,1ERR) 
CALL DSTABL(DEOS,WR,WI,FREQ) 


DEOSM=DEOS 

ADM=XD 

PRL=DEOSL*DEOSM 
PRR=DEOSR* DEOSM 
MieenieG?.0.D0) GO TO 5 
ADO=XDL 


3 


XDN=XDM 
DEOSO=DEOSL 
DEOSN=DEOSM 
Pe — ieee 
IF (IL.GT.ILMAX) STOP 3100 
DIF=DABS (XDL-XDM) 
IF (DIF .GT. EPs) GOs tema 
XD=XDM 
GOR Toma 
5 IF (PRR.GT.0, DO) Steer s3sZ0G 
XDO=XDM 
XDN=XDR 
DEOSO=DEOSM 
DEOSN=DEOSR 
IL=IL+1 
IF (IL.GT. LLMAX) steers oe 
DIF=DABS (XDM-XDR) 
IF (DIF. CGT VEPs) sco) lems 


XD=XDM 
4 LLL=10+LL 

WRITE “(LLL, *)) Dyas 
3 XDOO=XDNN 


DEOSOO=DEOSNN 
2 CONTINUE 

COMO 
23 IF (VAL1.GT.0.0) WRITE (11,*) Dw ony 
IF (VAL2.GT.0.0) WRITE (12,*) 92m) UV 

1 CONTINUE 
IF (ZG.NE.0.0) WRITE (10,*) XDMIN/L,TVCR1 
IF (ZG.NE.0.0) WRITE (10,*) XDMAX/L,TVCRI1 


1001 FORMAT (' ENTER MIN, MAX, AND INCREMENTS OF TV') 
1002 FORMAT (' ENTER MIN, MAX, AND INCREMENTS OF XD') 
1003 FORMAT (' ENTER U AND 2G') 
1004 FORMAT (' ENTER 0 : NUMERICAL',/, 

& ' 1 : ANALYTICAL ' ) 
2001 FORMAT (215) 

END 


SUBROUTINE DSTABL(DEOS,WR,WI,OMEGA ) 
IMPLICIT DOUBLE PRECISION (4-7 Oo =) 
DIMENSION WR(4),WI1(4) 
DEOS=-1.0D+20 
DO 1 I=1,4 
IF (WR( 1). ET. DEGS)cOmuen. 
DEOS=WR(T) 
IJ=I 
1 CONTINUE 
OMEGA=WI(IJ) 
OMEGA=DABS (OMEGA) 
RETURN 


94 


END 


FUNCTION CBRT(A) 
DECC r ORO GERT= memo 1. /3. ) 
HELE. 0.0) «CBRT=— (my **(1./3.) 
RETURN 

END 


o5 


OC Oe @ 


APPENDIX C 


PROGRAM VERT STEADY. Ok 


COMPUTATION OF STEADY STATE SOLUTIONS IN THE VERTICAL 
(CHAPTER III, 


REAL K1V,K2V,K3V,L,MQDOT,MQ,MW,MWDOT,MDS,MDB,MASS, IY 


WE IGHT=12000.0 

IN = 945020 

L = Ls aZo 
RHO = ee 

G = Bia 

XG = 0.0 

ZB = GOF.0 

MASS =WEIGHT/G 

BOY =WEIGHT 

ZODOT =-6.810E-03%*0. 
ZWDOT =-2.430E-01*0 
ZQ =—1 . 3505-0170 
ZW =-3.020E-01*0 
ZDS =-2.270E-02*0 
ZDB =-2.270E-02*0. 
MODOT =-1.680E-02*0 
MWDOT =-6.810E-02*0 
MOQ =-6.860E-02*0 
MW = 9.860H-02*0 
MDS =-1.113E-02*0 
MDB = 1.113E-02*0 
OPEN 

OPEN 

OPEN 

OPEN 

OPEN 

OPEN 

SAT =0.4 

SATP= SAT 

SATM=-SAT 


PI =4.0*ATAN(1.0) 


PLANE 


PARAGRAPH F) 


> RHO ips 


oO hehe 
7 Oy eee 
«o ~RHOs Tie 
» RHO * eee 


S * Rha sez 


Oo =RHO- hss 
-o 7 REO elie 
io *RHCai 4 
9 * RAC ae 
1D “RO ae 
Res) ess olObes <3 


(11,FILE='THETA].RES”’, STAggs— NEW 
(12, FILE='THETA2.RES' , STATUS='NEW' ) 
(13,FILE='THETA3.RES' , STATUS='NEW’ ) 
(14,FILE='THETA4.RES' ,STATUS='NEW' ) 
(21,FILE='DELTA1.RES' , STATUS='NEW' ) 
(22, FILE='DELTA2.RES' , STATUS='NEW'’ ) 
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10 


20 


30 


Ios. 


WRITE 
READ 
GO TO 
WRITE 
READ 


(*,1001) 

(*,*) IVAR 
(10,20,30), IVAR 

(*, 1002) 

(*,*) UMIN, UMAX, IU 


INCR=IU 


WRITE 
READ 
WRITE 
READ 
GO TO 
WRITE 
READ 


(ea) ONG 
Ce) TV 


(*,1004) 
(*,*) ZGMIN, ZGMAX, IZG 


INCR=12ZG 


WRITE 
READ 
WRITE 
READ 
GO TO 
WRITE 
READ 


(*,1005) 
ome U 

(*,1006) 

(*,*) TV 

15 

(*,1007) 

(*,*) TVMIN, TVMAX, ITV 


INCR=ITV 


WRITE 
READ 
WRITE 
READ 


DO 1 
Er 
Le 
ee 
DV 


(*, 1003) 
Gay ) ZG 
(*,1005) 

Ca) U 


I=1,INCR 
(IVAR.EQ.1) U =UMIN +(UMAX -UMIN )*(I-1)/(INCR-1) 
(IVAR.EQ.2) ZG=ZGMIN+(ZGMAX-ZGMIN)*(I-1)/(INCR-1) 
(IVAR.EQ.3) TV=TVMIN+(TVMAX-TVMIN) *(I-1)/(INCR-1) 
=(MASS-ZWDOT) * (IY-MQDOT ) -ZQDOT*MWDOT 


Al1V=((IY-MQDOT) *ZW+ZQDOT*MW) /DV 
Al2V=((IY-MODOT) *(ZQ+MASS )+ZQDOT*MQ) /DV 
A13V=-(ZG-ZB) *(MASS*XG+ZQDOT ) *WEIGHT/DV 
A21V=(MWDOT* ZW+(MASS-ZWDOT ) *MW) /DV 
A22V=(MWDOT* (ZQ+MASS )+(MASS-ZWDOT) *MQ) /DV 
A23V=-(ZG-ZB) * (MASS-ZWDOT) *WEIGHT/DV 
Bl1V=((IY-MQDOT) *ZDS+ZQDOT*MDS ) /DV 
B12V=((IY-MQDOT ) *ZDB+ZQDOT*MDB) /DV 
B21V=(MWDOT* ZDS+(MASS-ZWDOT) *MDS ) /DV 
B22V=(MWDOT* ZDB+(MASS-ZWDOT) *MDB) /DV 
B1V =B11V-B1l2V 

B2V =B21V-B22V 


OMEGAV=(10.0*U)/(TV*L) 
AD1V=1.75*OMEGAV 
AD2V=2.15*OMEGAV* * 2 
AD3V=OMEGAV* * 3 


A2= 
A3= 


Bly U* U 
Bay Ue 


an 


D1=-ADIV-(AllV+A22y 0 

B1=-B2V*U*U 
B2=(B1V*A22V-B2V*A12V) *U**3 
B3=(B2V*A11V-B1V*A21V) *U**3 
D2=AD2V+A23V+(A12V*A21V-A11V*A22V) *U**2 
Cl=(B2V*A11V-B1V*A21V) *U**3 
C2=(A23V*B1V-Al13V*B2V) *U**2 
D3=AD3V+(A13V*A21V-A11V*A23V) *U 
K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 
K2V=K2V/ (A3*B1*C2+C1*B3*A2-C1*A3*B2) 
K1V=(D3-C2*K2vV) /Cl 
K3V=(D1-A2*K2V) /A3 


IF (IVAR.EQ.1) OUT=U 

IF (IVAR.EQ.2) OUT=2ZG 

IF (IVAR.EQ.3) OUT=TV 
D3P=(A13V*A21V-A11V*A23V) *U 
XAAA=CBRT(-D3P) 
TVCR=(10.*U) /(XAAA*L) 

IF (TV.LT IVER) Gonroel 


CALL SOLSET( INUM, THSOLS,K1V,C1,AD3V,SSTH) 

ICHECK=0 

DO 2 III=1,INUM 
THCH=2.0*(THSOLS-0.5*PI) 
CHECK=SIN(THCH) * (D3-AD3V)/Cl 
IF (ABS(CHECK).GT.SATP) GO TO 2 
WRITE (13,*) OUT, THCH*180.0/PI 
WRITE (14,*) OUT,-THCH*180.0/PI 
WRITE (21,*) OUT, ABS(CHECK)*180.0/PI 
ICHECK=1 

2 CONTINUE 
IF (ICHECK.EQ.0) GO TO 3 
COurOel 


3. STHETA=SATP*C1/(D3-AD3V) 
SSTH=ASIN(STHETA) 
THETA1= SSTH*180.0/PI 
THETA2=-SSTH*180.0/PI 
WRITE (11,*) OUT, THETAI1 
WRITE (12,*) OUT, THETA2 
WRITE (22,*) OUT, SATP*180.0/PI 
1 CONTINUE 
STOP 
1001 FORMAT (' ENTER 1: U_ VARIATION',/, 
& : 2: ZG VARIATIONS. 
& | 3: TV VARIATION’ ) 
1002 FORMAT (' ENTER MIN, MAX, AND INCREMENTS IN U') 
1003 FORMAT (' ENTER ZG') 
1004 FORMAT (' ENTER MIN, MAX, AND INCREMENTS IN ZG') 
1005 FORMAT (' ENTER U') 
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c 


1006 FORMAT (' ENTER TV') 


1007 


EL 


10 


eZ 


FORMAT (' ENTER MIN, MAX, AND INCREMENTS IN TV') 


END 
FUNCTION CBRT(A) 

imeem On0) GBRT= A 2%]. /3.) 
nee eee). 0.0m CBRT=-(-A)**(1./3.) 
RETURN 

END 


SUBROUTINE SOLSET(L,ANS,K1V,C1,AD3V,SSTH) 
REAL K1V 
DIMENSION VF(1,2) 


PI=4.0*ATAN(1.0) 


FIND FIRST ESTIMATE OF THE SOLUTIONS 
i= 
VMIN= 0.0 
VMAX=+90.0 
IV=100 
VA=VMIN*PI/180.0 
VAO=VA 
VO=THETEQ(1,VA,K1V,C1,AD3V) 
DOmmOml=2 1 
VA=VMIN+(VMAX-VMIN) *(I-1)/(IV-1) 
VA=VA*PI/180.0 


VAN=VA 
VN=THETEQ(1,VA,K1V,C1,AD3V) 
VP=VO*VN 

IF (VP.GE.0.0) GO TO 11 
L=L+1 


Eee =v RO 
VF (L,2)=VAN 
GOnrO) 12 
VO=VN 
VAO=VAN 
CONTINUE 


EXACT COMPUTATION OF SOLUTIONS VIA NEWTON'S METHOD 
E=1.E-5 
IEND=500 
DO 20 J=1,L 
Keone jee d,2))/2.0 
F=THETEQ(1,X,K1V,C1,AD3V) 
FDER=THETEQ(2,X,K1V,C1,AD3V) 
DO 30 K=1,IEND 
IF (FDER.EQ.0.0) STOP 1001 
DX=F/FDER 
lx x 
F=THETEQ(1,X1,K1V,C1,AD3V) 


29 


40 
30 


39 
20 


10 


20 
50 


FDER=THETEQ(2,X1,K1V,C1,AD3V) 
IF (F.EQ.0.) GO TO 35 
A=ABS(X1-X) 
IF (A-E) 35,35,40 
X=X1 
CONTINUE 
GO TO 20 
ANS=X1 
CONTINUE 
RETURN 
END 


FUNCTION THETEQ(K, THETA, K1V,C1,AD3V) 

REAL K1V 

GO TO. ( Tomo x 

THETEQ=K1V*C1*THETA+ (AD3V-K1V*C1) *COS (THETA) 
GO TO 50 
THETEQ=K1V*C1-(AD3V-K1V*Cl1 ) *SIN( THETA) 
RETURN 

END 
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